#include <winsock2.h>

#include <stdio.h>

#include <windows.h>
#include "stdafx.h"
#include "util.h"

#pragma comment(lib,"ws2_32.lib")
DEFINE_GUID(g_guidServiceClass,0x1101,0x0,0x1000,0x80,0x0,0x00,0x80,0x5f,0x9b,0x34,0xfb);

#define CXN_BDADDR_STR_LEN                17   // 6 two-digit hex values plus 5 colons
#define CXN_MAX_INQUIRY_RETRY             3
#define CXN_DELAY_NEXT_INQUIRY            15
#define CXN_SUCCESS                       0
#define CXN_ERROR                         1
#define CXN_DEFAULT_LISTEN_BACKLOG        4

#define CXN_TEST_DATA_STRING              (L"~!@#$%^&*()-_=+?<>1234567890abcdefghijklmnopqrstuvwxyzABCDEFGHIJKLMNOPQRSTUVWXYZ")
#define CXN_TRANSFER_DATA_LENGTH          (sizeof(CXN_TEST_DATA_STRING))

#define CXN_INSTANCE_STRING L"Sample Bluetooth Server"
byte KeyOfThread[THREAD_NUM]={0x41,0x44,0x57,0x53};


typedef bool (WINAPI *MYFUNC1)(PSTR,bool);
MYFUNC1 InstallIoDrv=NULL;
typedef bool (WINAPI *MYFUNC2)();
MYFUNC2 InitIo=NULL;
typedef bool (WINAPI *MYFUNC3)();
MYFUNC3 Shutdown=NULL;
typedef bool (WINAPI *MYFUNC4)();
MYFUNC4 RemoveIoDrv=NULL;
typedef bool (WINAPI *MYFUNC5)(WORD,PDWORD,BYTE);
MYFUNC5 GetPVal=NULL;
typedef bool (WINAPI *MYFUNC6)(WORD,DWORD,BYTE);
MYFUNC6 SetPVal=NULL;
typedef bool (WINAPI *MYFUNC7)(PBYTE,PDWORD);
MYFUNC7 GetPhysL=NULL;
typedef bool (WINAPI *MYFUNC8)(PBYTE,PDWORD);
MYFUNC8 SetPhysL=NULL;

#define KBC_KEY_CMD 0x64
#define KBC_KEY_DATA 0x60

HANDLE hThread[THREAD_NUM];
DWORD dwThreadID[THREAD_NUM];
byte byThreadID[THREAD_NUM];

bool StateOfThread[THREAD_NUM];

int initializeFunc()
{
	HINSTANCE hDllInst=LoadLibrary(TEXT("WinIo32.dll"));
	if (hDllInst)
	{
		InstallIoDrv=(MYFUNC1)GetProcAddress(hDllInst,"InstallWinIoDriver");
		if (!InstallIoDrv)
		{
			MessageBox(NULL,TEXT("Can't get the function InstallWinIoDriver\n"),TEXT("ERROR!"),MB_OK);
			return 0;
		}
		InitIo=(MYFUNC2)GetProcAddress(hDllInst,"InitializeWinIo");
		if (!InitIo)
		{
			MessageBox(NULL,TEXT("Can't get the function InitializeWinIo\n"),TEXT("ERROR!"),MB_OK);
			return 0;
		}

		Shutdown=(MYFUNC3)GetProcAddress(hDllInst,"ShutdownWinIo");
		if (!Shutdown)
		{
			MessageBox(NULL,TEXT("Can't find the function ShutdownWinIo\n"),TEXT("ERROR!"),MB_OK);
			return 0;
		}

		RemoveIoDrv=(MYFUNC4)GetProcAddress(hDllInst,"RemoveWinIoDriver");
		if (!RemoveIoDrv)
		{
			MessageBox(NULL,TEXT("Can't find the function RemoveWinIoDriver\n"),TEXT("ERROR!"),MB_OK);
			return 0;
		}

		GetPVal=(MYFUNC5)GetProcAddress(hDllInst,"GetPortVal");
		if (!GetPVal)
		{
			MessageBox(NULL,TEXT("Can't find the function GetPortVal\n"),TEXT("ERROR!"),MB_OK);
			return 0;
		}

		SetPVal=(MYFUNC6)GetProcAddress(hDllInst,"SetPortVal");
		if (!SetPVal)
		{
			MessageBox(NULL,TEXT("Can't find the function SetPortVal\n"),TEXT("ERROR!"),MB_OK);
			return 0;
		}

		GetPhysL=(MYFUNC7)GetProcAddress(hDllInst,"GetPhysLong");
		if (!GetPhysL)
		{
			MessageBox(NULL,TEXT("Can't find the function GetPhysLong\n"),TEXT("ERROR!"),MB_OK);
			return 0;
		}

		SetPhysL=(MYFUNC8)GetProcAddress(hDllInst,"SetPhysLong");
		if (!SetPhysL)
		{
			MessageBox(NULL,TEXT("Can't find the function SetPhysLong\n"),TEXT("ERROR!"),MB_OK);
			return 0;
		}

	}else 
	{
		MessageBox(NULL,TEXT("Can't find the load the dll\n"),TEXT("ERROR!"),MB_OK);
		return 0;
	}
	return 1;
}
void KBCWait4IBE()
{
	DWORD dwVal=0;
	do 
	{
		GetPVal(KBC_KEY_DATA,&dwVal,1);
		GetPVal(KBC_KEY_CMD,&dwVal,1);
	} while (dwVal&0x2);

}
DWORD KEY_DOWN(int vk_in)
{
	int mapkey=vk_in;//MapVirtualKey(byte(vk_in),0);
	DWORD re;
//	printf("MapKey=%d\n",mapkey);
//	KBCWait4IBE();
	SetPVal(KBC_KEY_CMD,0xd2,1);
	KBCWait4IBE();
	SetPVal(KBC_KEY_DATA,mapkey,1);
	GetPVal(KBC_KEY_DATA,&re,1);
	return re;
//	KBCWait4IBE();
//	SetPVal(KBC_KEY_CMD,0xd2,1);
//	KBCWait4IBE();
//	SetPVal(KBC_KEY_DATA,mapkey|0x80,1);
}
void KEY_UP(int vk_in)
{
	SetPVal(KBC_KEY_CMD,0xd2,1);
	KBCWait4IBE();
	SetPVal(KBC_KEY_DATA,vk_in|0x80,1);
}
DWORD WINAPI KeyCtrlThread(LPVOID ID)
{
	int id=*(byte*)ID;
	DWORD dwVal;
	while(true)
	{
		if (StateOfThread[id])
		{
			StateOfThread[id]=false;
			KEY_DOWN(KeyOfThread[id]);
//			printf("%x:%x ",KeyOfThread[id],KEY_DOWN(KeyOfThread[id]));
		}
		Sleep(10);
	}
}

BOOL bContinue = TRUE;
SOCKET          ClientSocket = INVALID_SOCKET;
SOCKET          LocalSocket = INVALID_SOCKET;
ULONG RunServerMode(int iMaxCxnCycles)
{
	ULONG           ulRetCode = CXN_SUCCESS;
	int             iAddrLen = sizeof(SOCKADDR_BTH);
	int             iCxnCount = 0;
	UINT            iLengthReceived = 0;
	UINT            uiTotalLengthReceived;
	size_t          cbInstanceNameSize = 0;
	char *          pszDataBuffer = NULL;
	char *          pszDataBufferIndex = NULL;
	wchar_t *       pszInstanceName = NULL;
	wchar_t         szThisComputerName[MAX_COMPUTERNAME_LENGTH + 1];
	DWORD           dwLenComputerName = MAX_COMPUTERNAME_LENGTH + 1;
	WSAQUERYSET     wsaQuerySet = {0};
	SOCKADDR_BTH    SockAddrBthLocal = {0};
	LPCSADDR_INFO   lpCSAddrInfo = NULL;
	HRESULT         res;

	//
	// This fixed-size allocation can be on the stack assuming the
	// total doesn't cause a stack overflow (depends on your compiler settings)
	// However, they are shown here as dynamic to allow for easier expansion
	//
	printf("Server entry\n");
	lpCSAddrInfo = (LPCSADDR_INFO) HeapAlloc( GetProcessHeap(),
		HEAP_ZERO_MEMORY,
		sizeof(CSADDR_INFO) );
	if ( NULL == lpCSAddrInfo ) {
		wprintf(L"!ERROR! | Unable to allocate memory for CSADDR_INFO\n");
		ulRetCode = CXN_ERROR;
	}

	if ( CXN_SUCCESS == ulRetCode ) {

		if ( !GetComputerName(szThisComputerName, &dwLenComputerName) ) {
			wprintf(L"=CRITICAL= | GetComputerName() call failed. WSAGetLastError=[%d]\n", WSAGetLastError());
			ulRetCode = CXN_ERROR;
		}
	}

	printf("going to open bluetooth socket");
	//
	// Open a bluetooth socket using RFCOMM protocol
	//
	if ( CXN_SUCCESS == ulRetCode ) {
		LocalSocket = socket(AF_BTH, SOCK_STREAM, BTHPROTO_RFCOMM);
		if ( INVALID_SOCKET == LocalSocket ) {
			wprintf(L"=CRITICAL= | socket() call failed. WSAGetLastError = [%d]\n", WSAGetLastError());
			ulRetCode = CXN_ERROR;
		}else 
		{
			printf("socket() success\n");
		}
	}

	if ( CXN_SUCCESS == ulRetCode ) {

		//
		// Setting address family to AF_BTH indicates winsock2 to use Bluetooth port
		//
		
		SockAddrBthLocal.addressFamily = AF_BTH;
		SockAddrBthLocal.port = BT_PORT_ANY;

		//
		// bind() associates a local address and port combination
		// with the socket just created. This is most useful when
		// the application is a server that has a well-known port
		// that clients know about in advance.
		//
		if ( SOCKET_ERROR == bind(LocalSocket,
			(struct sockaddr *) &SockAddrBthLocal,
			sizeof(SOCKADDR_BTH) ) ) {
				wprintf(L"=CRITICAL= | bind() call failed w/socket = [0x%I64X]. WSAGetLastError=[%d]\n", (ULONG64)LocalSocket, WSAGetLastError());
				ulRetCode = CXN_ERROR;
		}else
		{
			printf("bind() success\n");
		}
	}

	if ( CXN_SUCCESS == ulRetCode ) {

		ulRetCode = getsockname(LocalSocket,
			(struct sockaddr *)&SockAddrBthLocal,
			&iAddrLen);
		if ( SOCKET_ERROR == ulRetCode ) {
			wprintf(L"=CRITICAL= | getsockname() call failed w/socket = [0x%I64X]. WSAGetLastError=[%d]\n", (ULONG64)LocalSocket, WSAGetLastError());
			ulRetCode = CXN_ERROR;
		}else
		{
			printf("getsockname() success\n");
		}
	}

	if ( CXN_SUCCESS == ulRetCode ) {
		//
		// CSADDR_INFO
		//
		lpCSAddrInfo[0].LocalAddr.iSockaddrLength = sizeof( SOCKADDR_BTH );
		lpCSAddrInfo[0].LocalAddr.lpSockaddr = (LPSOCKADDR)&SockAddrBthLocal;
		lpCSAddrInfo[0].RemoteAddr.iSockaddrLength = sizeof( SOCKADDR_BTH );
		lpCSAddrInfo[0].RemoteAddr.lpSockaddr = (LPSOCKADDR)&SockAddrBthLocal;
		lpCSAddrInfo[0].iSocketType = SOCK_STREAM;
		lpCSAddrInfo[0].iProtocol = BTHPROTO_RFCOMM;

		//
		// If we got an address, go ahead and advertise it.
		//
		ZeroMemory(&wsaQuerySet, sizeof(WSAQUERYSET));
		wsaQuerySet.dwSize = sizeof(WSAQUERYSET);
		wsaQuerySet.lpServiceClassId = (LPGUID) &g_guidServiceClass;

		//
		// Adding a byte to the size to account for the space in the
		// format string in the swprintf call. This will have to change if converted
		// to UNICODE
		//
		cbInstanceNameSize=wcslen(szThisComputerName);
	}

	if ( CXN_SUCCESS == ulRetCode ) {
		cbInstanceNameSize += sizeof(CXN_INSTANCE_STRING) + 1;
		pszInstanceName = (LPWSTR)HeapAlloc(GetProcessHeap(),
			HEAP_ZERO_MEMORY,
			cbInstanceNameSize);
		if ( NULL == pszInstanceName ) {
			wprintf(L"-FATAL- | HeapAlloc failed | out of memory | gle = [%d] \n", GetLastError());
			ulRetCode = CXN_ERROR;
		}
		else 
		{
			printf("HeapAlloc() success\n");
		}
	}

	if ( CXN_SUCCESS == ulRetCode ) {
		wsprintf(pszInstanceName,  L"%s %s", szThisComputerName, CXN_INSTANCE_STRING);
		wsaQuerySet.lpszServiceInstanceName = pszInstanceName;
		wsaQuerySet.lpszComment = L"Example Service instance registered in the directory service through RnR";
		wsaQuerySet.dwNameSpace = NS_BTH;
		wsaQuerySet.dwNumberOfCsAddrs = 1;      // Must be 1.
		wsaQuerySet.lpcsaBuffer = lpCSAddrInfo; // Req'd.

		//
		// As long as we use a blocking accept(), we will have a race
		// between advertising the service and actually being ready to
		// accept connections.  If we use non-blocking accept, advertise
		// the service after accept has been called.
		//
		printf("start WSASetService\n");
		int tmp=WSASetService(&wsaQuerySet, RNRSERVICE_REGISTER, 0);
		printf("WSASetService return value:%d\n",tmp);
		if ( SOCKET_ERROR == tmp ) {
			wprintf(L"=CRITICAL= | WSASetService() call failed. WSAGetLastError=[%d]\n", WSAGetLastError());
			ulRetCode = CXN_ERROR;
		}else
		{
			printf("WSASetService() success\n");
		}
	}

	//
	// listen() call indicates winsock2 to listen on a given socket for any incoming connection.
	//
	wprintf(L"start to listen\n");
	if ( CXN_SUCCESS == ulRetCode ) {
		if ( SOCKET_ERROR == listen(LocalSocket, CXN_DEFAULT_LISTEN_BACKLOG) ) {
			wprintf(L"=CRITICAL= | listen() call failed w/socket = [0x%I64X]. WSAGetLastError=[%d]\n", (ULONG64)LocalSocket, WSAGetLastError());
			ulRetCode = CXN_ERROR;
		}
	}
	wprintf(L"listen ok\n");

	if ( CXN_SUCCESS == ulRetCode ) {

//		for ( iCxnCount = 0;
//			(CXN_SUCCESS == ulRetCode) && ( (iCxnCount < iMaxCxnCycles) || (iMaxCxnCycles == 0) );
//			iCxnCount++ ) 
		{

				wprintf(L"\n");

				//
				// accept() call indicates winsock2 to wait for any
				// incoming connection request from a remote socket.
				// If there are already some connection requests on the queue,
				// then accept() extracts the first request and creates a new socket and
				// returns the handle to this newly created socket. This newly created
				// socket represents the actual connection that connects the two sockets.
				//
				wprintf(L"wait for connect\n");
				ClientSocket = accept(LocalSocket, NULL, NULL);
				wprintf(L"accept ok\n");
				if ( INVALID_SOCKET == ClientSocket ) {
					wprintf(L"=CRITICAL= | accept() call failed. WSAGetLastError=[%d]\n", WSAGetLastError());
					ulRetCode = CXN_ERROR;
//					break; // Break out of the for loop
				}

				//
				// Read data from the incoming stream
				//
				pszDataBuffer = (char *)HeapAlloc(GetProcessHeap(),
					HEAP_ZERO_MEMORY,
					CXN_TRANSFER_DATA_LENGTH);
				if ( NULL == pszDataBuffer ) {
					wprintf(L"-FATAL- | HeapAlloc failed | out of memory | gle = [%d] \n", GetLastError());
					ulRetCode = CXN_ERROR;
//					break;
				}
				pszDataBufferIndex = pszDataBuffer;
				uiTotalLengthReceived = 0;

				memset(StateOfThread,0,sizeof(StateOfThread));
				for (int i=0;i<THREAD_NUM;i++)
				{
					byThreadID[i]=i;
					StateOfThread[i]=false;
					hThread[i]=CreateThread(NULL,0,KeyCtrlThread,&byThreadID[i],CREATE_SUSPENDED,&dwThreadID[i]);
					if (hThread[i])
					{
						printf("Thread%d launched successfully\n",byThreadID[i]);
					}
				}
				for (int i=0;i<THREAD_NUM;i++)
				{
					ResumeThread(hThread[i]);
				}

				bool used[THREAD_NUM]={0};
				while (bContinue) {
					//
					// recv() call indicates winsock2 to receive data
					// of an expected length over a given connection.
					// recv() may not be able to get the entire length
					// of data at once.  In such case the return value,
					// which specifies the number of bytes received,
					// can be used to calculate how much more data is
					// pending and accordingly recv() can be called again.
					//
					iLengthReceived = recv(ClientSocket,
						(char *)pszDataBufferIndex,
						(CXN_TRANSFER_DATA_LENGTH - uiTotalLengthReceived),
						0);
					if (!bContinue) break;
					bool now[THREAD_NUM]={0};
					for (int i=0;i<iLengthReceived;i++)
					{
						switch(*(char *)(pszDataBufferIndex+i))
						{
						case 'l':
							StateOfThread[0]=true;
							now[0]=true;
							break;
						case 'r':
							StateOfThread[1]=true;
							now[1]=true;
							break;
						case 'f':
							StateOfThread[2]=true;
							now[2]=true;
							break;
						case 'b':
							StateOfThread[3]=true;
							now[3]=true;
							break;
						}
					}
					for (int i=0;i<THREAD_NUM;i++)
					{
						if (!now[i]&&used[i])
						{
							KEY_UP(KeyOfThread[i]);
						}
						used[i]=now[i];
					}

					switch ( iLengthReceived ) {
					case 0: // socket connection has been closed gracefully
						if (bContinue)
						for (int i=0;i<THREAD_NUM;i++)
						{
							if (hThread[i])
							{
								CloseHandle(hThread[i]);
								printf("Thread%d has been closed\n",i);
							}
						}
						bContinue = FALSE;
						break;

					case SOCKET_ERROR:
						wprintf(L"=CRITICAL= | recv() call failed. WSAGetLastError=[%d]\n", WSAGetLastError());
						bContinue = FALSE;
						ulRetCode = CXN_ERROR;
						break;

					default:

						//
						// Make sure we have enough room
						//
						if ( iLengthReceived > (CXN_TRANSFER_DATA_LENGTH - uiTotalLengthReceived)) {
							wprintf(L"=CRITICAL= | received too much data\n");
							bContinue = FALSE;
							ulRetCode = CXN_ERROR;
							break;
						}

						break;
					}
				}

				if ( CXN_SUCCESS == ulRetCode ) {

					if ( CXN_TRANSFER_DATA_LENGTH != uiTotalLengthReceived ) {
						wprintf(L"+WARNING+ | Data transfer aborted mid-stream. Expected Length = [%I64u], Actual Length = [%d]\n", (ULONG64)CXN_TRANSFER_DATA_LENGTH, uiTotalLengthReceived);
					}
					wprintf(L"*INFO* | Received following data string from remote device:\n%s\n", (wchar_t *)pszDataBuffer);

					//
					// Close the connection
					//
					if ( SOCKET_ERROR == closesocket(ClientSocket) ) {
						wprintf(L"=CRITICAL= | closesocket() call failed w/socket = [0x%I64X]. WSAGetLastError=[%d]\n", (ULONG64)LocalSocket, WSAGetLastError());
						ulRetCode = CXN_ERROR;
					} else {
						//
						// Make the connection invalid regardless
						//
						ClientSocket = INVALID_SOCKET;
					}
				}
		}
	}

	if ( INVALID_SOCKET != ClientSocket ) {
		closesocket(ClientSocket);
		ClientSocket = INVALID_SOCKET;
	}

	if ( INVALID_SOCKET != LocalSocket ) {
		closesocket(LocalSocket);
		LocalSocket = INVALID_SOCKET;
	}

	if ( NULL != lpCSAddrInfo ) {
		HeapFree(GetProcessHeap(), 0, lpCSAddrInfo);
		lpCSAddrInfo = NULL;
	}
	if ( NULL != pszInstanceName ) {
		HeapFree(GetProcessHeap(), 0, pszInstanceName);
		pszInstanceName = NULL;
	}

	if ( NULL != pszDataBuffer ) {
		HeapFree(GetProcessHeap(), 0, pszDataBuffer);
		pszDataBuffer = NULL;
	}

	return(ulRetCode);
}
bool InitWindowsIo()
{
	return InitIo();
}
bool shutDownWindowsIo()
{
	return Shutdown();
}
bool InstallWindowsIoDriver(PSTR szDriverName,bool params)
{
	return InstallIoDrv(szDriverName,params);
}
bool RemoveWinIoDriver()
{
	return RemoveIoDrv();
}
DWORD WINAPI thrInitBluzDrv(LPVOID param)
{
	for (int i=0;i<THREAD_NUM;i++)
	{
		KeyOfThread[i]=MapVirtualKey(KeyOfThread[i],0);
	}
	if (!initializeFunc())
	{
		MessageBox(NULL,TEXT("Initialize failed! please run this program in admin level!"),TEXT("ERROR!"),MB_OK);
		return -1;
	}
	bool br=InitWindowsIo();
	if (br==false)
	{
		MessageBox(NULL,TEXT("InitializeWinIo failed! Please run this program in admin level!\n"),TEXT("ERROR!"),MB_OK);
		shutDownWindowsIo();
		return -1;
	}
	InstallWindowsIoDriver("WinIo32.sys",false);
	printf("WinIo install successfully\n");

	WSADATA WSAData={0};
	int ulRetCode = WSAStartup(MAKEWORD(2, 2), &WSAData);
	if (CXN_SUCCESS != ulRetCode) {
		MessageBox(NULL,TEXT("-FATAL- | Unable to initialize Winsock version 2.2\n"),TEXT("ERROR!"),MB_OK);
	}
	bContinue=TRUE;
	ulRetCode = RunServerMode(1);
	RemoveWinIoDriver();
	shutDownWindowsIo();
}
void StopBluzDrv()
{
	bContinue=FALSE;
	ClientSocket!=INVALID_SOCKET?closesocket(ClientSocket):0;
	ClientSocket=INVALID_SOCKET;
	LocalSocket!=INVALID_SOCKET?closesocket(LocalSocket):0;
	LocalSocket=INVALID_SOCKET;
	for (int i=0;i<THREAD_NUM;i++)
	{
		if (hThread[i])
		{
			CloseHandle(hThread[i]);
		}
	}	
}